
#ifndef AXIS_H
#define AXIS_H

#include "stepmotor.h"

typedef struct axis_tag{
	stepmotor motor;
	int current_pos;
	int goal_pos;
	int inv_velocity;   // inverse of velocity [control cycle / one motor step]
	int cycle_count;
	BOOL in_position;   // true if current_pos == goal_pos
} axis;


void axis_init(axis* axis){
	stepmotor_init(&axis->motor);
	axis->current_pos = 0;
	axis->goal_pos = 0;
	axis->inv_velocity = 1;
	axis->cycle_count = 0;
	axis->in_position = TRUE;
}


void axis_set_goal_pos(axis* axis, int goal_pos){
	axis->goal_pos = goal_pos;
}

void axis_set_inv_velocity(axis* axis, int inv_velocity){
	axis->inv_velocity = inv_velocity;
}

void axis_do_one_control_cycle(axis* axis){
	if(axis->inv_velocity > 0){
		if(axis->cycle_count >= axis->inv_velocity){
			if(axis->current_pos == axis->goal_pos){
				axis->in_position = TRUE;
			}
			else{
				axis->in_position = FALSE;
				if(axis->current_pos > axis->goal_pos){
					stepmotor_rotate(&axis->motor, -1);
				}
				else if(axis->current_pos < axis->goal_pos){
					stepmotor_rotate(&axis->motor, 1);
				}
				axis->current_pos = axis->motor.pos;
			}
			axis->cycle_count = 0;
		}
		axis->cycle_count++;
	}
}


void axis_enable(axis* axis){
	axis->motor.enable = TRUE;
}

void axis_disenable(axis* axis){
	axis->motor.enable = FALSE;
}


#endif

